/**
 * @file gps_node.cpp
 * @brief GPS节点，解析GPGGA数据
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-05-18
 * 
 * @copyright Copyright (c) 2020  WHU-EIS
 * 
 */

#include <iostream>
#include <string>
#include <sstream>
#include <fstream>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <sensor_msgs/Imu.h>

#include <math.h>
#include <eigen3/Eigen/StdVector>

#include <globalplanner/BLH2locpos.hpp>

#include <thread>
#include <visualization_msgs/MarkerArray.h>

using al_topo_map::BLH2locpos;

std::vector<std::string> split(const std::string& str, const char pattern){
	std::vector<std::string> res;
	std::stringstream input(str);
	std::string temp;
	while(getline(input, temp, pattern)){
		res.push_back(temp);
	}	
	return res;
}

template <class Type>
Type stringToNum(const std::string& str)
{
  std::istringstream iss(str);
  Type num;
  iss >> num;
  return num;
}

serial::Serial ser;

geometry_msgs::Twist twist;

int main(int argc, char** argv){
    ros::init(argc, argv, "gps_node");
	ros::NodeHandle nh;

	ros::NodeHandle n("~");
	std::string serial_port;
	int serial_baudrate;
	n.param("serial_port", serial_port, std::string("/dev/ttyUSB1"));
	n.param("serial_baudrate", serial_baudrate, 115200);

	ros::Publisher rtk_gps_pub = nh.advertise<sensor_msgs::NavSatFix>("rtk_gps", 50);  // 回传给小黑
	ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50); // 用于导航

	ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::Twist>("/tank/vel", 10, [&](const geometry_msgs::Twist::ConstPtr& msgs){
		twist.linear.x = msgs->linear.x;
		twist.angular.z = msgs->angular.z;
	});

	std::string path_points_path; // 路径点保存的路径
	n.param("path_points_path", path_points_path, std::string(""));
	ROS_INFO("path_points_path: %s", path_points_path.c_str());

	BLH2locpos blh2locpos;
	nav_msgs::Odometry odom;
	geometry_msgs::TransformStamped odom_trans;
	tf::TransformBroadcaster odom_broadcaster;

	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	// 清空串口缓存区
	ser.read(ser.available());

	// 组合导航模块状态
	int8_t gps_status = 0;

	ros::Rate loop_rate(100);
	while(ros::ok()){
		if(ser.available())
		{
			std::string data_receice = ser.readline();
			// ROS_INFO("%s", data_receice.c_str());
			std::vector<std::string> datas = split(data_receice, ',');
			
			// GPS定位结果
			if(datas[0] == "#BESTPOSA"){
				// sensor_msgs::NavSatFix nav_sat_fix;
				// sensor_msgs::NavSatStatus gps_status_;
				// gps_status_.status = gps_status;
				// nav_sat_fix.header.stamp = ros::Time::now();
				// nav_sat_fix.header.frame_id = "rtk_gps_link";
				// nav_sat_fix.status = gps_status_;
				// nav_sat_fix.latitude = stringToNum<double>(datas[11]);
				// nav_sat_fix.longitude = stringToNum<double>(datas[12]);				
				// nav_sat_fix.altitude = stringToNum<double>(datas[13]);
				// ROS_INFO("BESTPOSA: longitude: %f, latitude: %f", nav_sat_fix.longitude, nav_sat_fix.latitude);
				// rtk_gps_pub.publish(nav_sat_fix);
			}
			
			// 组合导航定位结果
			if(datas[0] == "#INSPVAXA"){
				
				std::vector<std::string> gps_status_str = split(datas[9].c_str(), ';');
				
				if(gps_status_str.size() == 2){
					ROS_INFO("%s", gps_status_str[1].c_str());
					if(gps_status_str[1] == "INS_INACTIVE"){
						gps_status = 0;
					}else if(gps_status_str[1] == "WAITING_INITIALPOS"){
						gps_status = 1;
					}else if(gps_status_str[1] == "WAITING_AZIMUTH"){
						gps_status = 2;
					}else if(gps_status_str[1] == "INS_ALIGNING"){
						gps_status = 3;
					}else if(gps_status_str[1] == "INS_ALIGNMENT_COMPLETE"){
						gps_status = 4;
					}else if(gps_status_str[1] == "INS_SOLUTION_GOOD"){
						gps_status = 5;
					}else if(gps_status_str[1] == "INS_HIGH_VARIANCE"){
						gps_status = 6;
					}else if(gps_status_str[1] == "INS_SOLUTION_FREE"){
						gps_status = 7;
					}
				}
				double longitude, latitude, altitude, roll, pitch, yaw, linear_x, linear_y, linear_z;
				latitude = stringToNum<double>(datas[11]);
				longitude = stringToNum<double>(datas[12]);
				altitude = stringToNum<double>(datas[13]);
				// ROS_INFO("INSPVAXA: longitude: %f, latitude: %f", longitude, latitude);

				sensor_msgs::NavSatFix nav_sat_fix;
				sensor_msgs::NavSatStatus gps_status_;
				gps_status_.status = gps_status;
				nav_sat_fix.header.stamp = ros::Time::now();
				nav_sat_fix.header.frame_id = "rtk_gps_link";
				nav_sat_fix.status = gps_status_;
				nav_sat_fix.latitude = latitude;
				nav_sat_fix.longitude = longitude;				
				nav_sat_fix.altitude = altitude;
				ROS_INFO("BESTPOSA: longitude: %f, latitude: %f", nav_sat_fix.longitude, nav_sat_fix.latitude);
				rtk_gps_pub.publish(nav_sat_fix);

				// 纬经高转换到xy坐标系
				Eigen::Vector3d blh;
				blh << latitude * M_PI / 180.0, longitude * M_PI / 180.0, altitude; 
				// blh << 24.61135864 * M_PI / 180.0, 118.30203225 * M_PI / 180.0, altitude;
				Eigen::Vector3d xy;
				blh2locpos.BLH2xy(blh, xy);

				linear_x = stringToNum<double>(datas[15]);
				linear_y = stringToNum<double>(datas[16]);
				linear_z = stringToNum<double>(datas[17]);
				
				roll = stringToNum<double>(datas[18]);
				pitch = stringToNum<double>(datas[19]);
				yaw = -1.0 * stringToNum<double>(datas[20]);
				// ROS_INFO("INSPVAXA: roll: %f, pitch: %f yaw: %f", roll, pitch, yaw);

				// 发布坐标变换
				geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw / 180.0 * M_PI);
				
				odom_trans.header.stamp = ros::Time::now();
				odom_trans.header.frame_id = "odom";
				odom_trans.child_frame_id = "base_link";

				odom_trans.transform.translation.x = xy.x();
				odom_trans.transform.translation.y = xy.y();
				odom_trans.transform.translation.z = 0.0;
				odom_trans.transform.rotation = odom_quat;
				//send the transform
				odom_broadcaster.sendTransform(odom_trans);

				// 发布里程计数据
				odom.header.stamp = ros::Time::now();
				odom.header.frame_id = "odom";

				//set the position
				odom.pose.pose.position.x = xy.x();
				odom.pose.pose.position.y = xy.y();
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = odom_quat;

				//set the velocity
				odom.child_frame_id = "base_link";
				odom.twist.twist = twist;
				
				odom_pub.publish(odom);


			}

		}
		ros::spinOnce();
		loop_rate.sleep();
	}
}